/**
 *  Encoder example code using only software interrupts
 * 
 * This is a code intended to test the encoder connections and to 
 * demonstrate the encoder setup fully using software interrupts.
 * - We use PciManager library: https://github.com/prampec/arduino-pcimanager
 * 
 * This code will work on Arduino devices but not on STM32 devices
 * 
 */

#include <SimpleFOC.h>
// software interrupt library
#include <PciManager.h>
#include <PciListenerImp.h>

// encoder instance
Encoder encoder = Encoder(A0, A1, 2048);

// interrupt routine intialisation
void doA()
{ encoder.handleA(); }

void doB()
{ encoder.handleB(); }

// encoder interrupt init
PciListenerImp listenerA(encoder.pinA, doA);
PciListenerImp listenerB(encoder.pinB, doB);

void setup()
{
    // monitoring port
    Serial.begin(115200);

    // enable/disable quadrature mode
    encoder.quadrature = Quadrature::ON;

    // check if you need internal pullups
    encoder.pullup = Pullup::USE_EXTERN;

    // initialise encoder hardware
    encoder.init();

    // interrupt initialization
    PciManager.registerListener(&listenerA);
    PciManager.registerListener(&listenerB);

    Serial.println("Encoder ready");
    _delay(1000);
}

void loop()
{
    // display the angle and the angular velocity to the terminal
    Serial.print(encoder.getAngle());
    Serial.print("\t");
    Serial.println(encoder.getVelocity());
}
